//
// Created by sri01 on 2025/11/4.
//

#include "MotorBLDC.h"
#include "bsp_adc.h"
#include "bsp_tim.h"

// 电流输入
static I_adc_t GetCurData(ADC_HandleTypeDef* hadc)
{
    I_adc_t I_adc;
    uint16_t Ia = hadc->Instance->JDR3;
    uint16_t Ib = hadc->Instance->JDR2;
    uint16_t Ic = hadc->Instance->JDR1;
    I_adc.i_a = (((float)Ia - 2048) * (3.3f / 4095.0f)) / (0.001f * 20.0f);
    I_adc.i_b = (((float)Ib - 2048) * (3.3f / 4095.0f)) / (0.001f * 20.0f);
    I_adc.i_c = (((float)Ic - 2048) * (3.3f / 4095.0f)) / (0.001f * 20.0f);
    return I_adc;
}

// 电压输出
static void Set_dtcPWM_abc(V_adc_t* V_adc)
{
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, (uint16_t)(V_adc->v_a * PWM_ARR));
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, (uint16_t)(V_adc->v_b * PWM_ARR));
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, (uint16_t)(V_adc->v_c * PWM_ARR));
}

/**
 * @brief FOC用户函数
 *
 * 此函数用于包装FOC的一些操作步骤
 *
 * @param BLDC 指向电机数据结构的指针
 */
void FOC_Current(Motor_Cur_t* Motor_Cur, float Id_set, float Iq_set)
{
    // Input
    Motor_Cur->BLDC_IN.I_adc = GetCurData(&hadc1);
    Motor_Cur->BLDC_IN.I_alphabeta = Clarke(&Motor_Cur->BLDC_IN.I_adc);
    switch(Motor_Cur->BLDC_Data.ElAngle_Get)
    {
        case Encoder_MT6816:
            GetFocMegAngle(&Motor_Cur->BLDC_Data.MT6816);
            Motor_Cur->Encoder.ElAngle = Motor_Cur->BLDC_Data.MT6816.Encoder.ElAngle;
            Motor_Cur->Encoder.ElAngle_vel = Motor_Cur->BLDC_Data.MT6816.Encoder.ElAngle_vel;
            break;
        case Encoder_Hall:
            FOC_HALL_Angle_Calc(&Motor_Cur->BLDC_Data.FOC_HALL);
            Motor_Cur->Encoder.ElAngle = Motor_Cur->BLDC_Data.FOC_HALL.Encoder.ElAngle;
            Motor_Cur->Encoder.ElAngle_vel = Motor_Cur->BLDC_Data.FOC_HALL.Encoder.ElAngle_vel;
            break;
        case Sensorless_HFT:
            FOC_HFI_Angle_Calc(&Motor_Cur->BLDC_Data.FOC_HFI, Motor_Cur->BLDC_IN.I_alphabeta);
            Motor_Cur->Encoder.ElAngle = Motor_Cur->BLDC_Data.FOC_HFI.Encoder.ElAngle;
            Motor_Cur->Encoder.ElAngle_vel = Motor_Cur->BLDC_Data.FOC_HFI.Encoder.ElAngle_vel;
            break;
        case Sensorless_SMO:
            FOC_SMO_Angle_Calc(&Motor_Cur->BLDC_Data.FOC_SMO, Motor_Cur->BLDC_IN.I_alphabeta,
                               Motor_Cur->BLDC_OUT.V_alphabeta);
            Motor_Cur->Encoder.ElAngle = Motor_Cur->BLDC_Data.FOC_SMO.Encoder.ElAngle;
            Motor_Cur->Encoder.ElAngle_vel = Motor_Cur->BLDC_Data.FOC_SMO.Encoder.ElAngle_vel;
            break;
        case Sensorless_VF:
            FOC_VF_Angle_Calc(&Motor_Cur->BLDC_Data.FOC_VF);
            Motor_Cur->Encoder.ElAngle = Motor_Cur->BLDC_Data.FOC_VF.Encoder.ElAngle;
            Motor_Cur->Encoder.ElAngle_vel = Motor_Cur->BLDC_Data.FOC_VF.Encoder.ElAngle_vel;
            break;
        default:
            break;
    }
    Motor_Cur->BLDC_Data.Circular = CORDIC_sin_cos(Motor_Cur->Encoder.ElAngle);
    Motor_Cur->BLDC_IN.I_dq = Park(&Motor_Cur->BLDC_IN.I_alphabeta, &Motor_Cur->BLDC_Data.Circular);

    // PI_Ctrl
    Motor_Cur->BLDC_OUT.V_dq.v_d = PID_Calc(&Motor_Cur->Pid_D, Motor_Cur->BLDC_IN.I_dq.i_d, Id_set);
    Motor_Cur->BLDC_OUT.V_dq.v_q = PID_Calc(&Motor_Cur->Pid_Q, Motor_Cur->BLDC_IN.I_dq.i_q, Iq_set);

    // Output
    float ElAngle_now = Motor_Cur->Encoder.ElAngle + Motor_Cur->Encoder.ElAngle_vel * CURRENT_MEASURE_PERIOD;
    Motor_Cur->BLDC_Data.Circular = CORDIC_sin_cos(ElAngle_now);
    Motor_Cur->BLDC_OUT.V_alphabeta = Inv_Park(&Motor_Cur->BLDC_OUT.V_dq, &Motor_Cur->BLDC_Data.Circular);
    Motor_Cur->BLDC_OUT.V_adc = Inv_clarke(&Motor_Cur->BLDC_OUT.V_alphabeta);
    Motor_Cur->BLDC_OUT.V_dtc = Svpwm_Sector(&Motor_Cur->BLDC_OUT.V_alphabeta);
    Motor_Cur->BLDC_OUT.V_dtc = Svpwm_Midpoint(&Motor_Cur->BLDC_OUT.V_alphabeta);
    Set_dtcPWM_abc(&Motor_Cur->BLDC_OUT.V_dtc);
}

Cur_Data_t GetMotorData(BLDC_Data_t* BLDC_Data, float Current)
{
    Cur_Data_t Cur_Data;
    Cur_Data.Position = BLDC_Data->MT6816.Encoder.MecAngle;
    Cur_Data.Speed = BLDC_Data->MT6816.Encoder.MecSpeed;
    Cur_Data.Current = Current;
    return Cur_Data;
}

uint8_t FOC_PosVel(Motor_App_t* Motor_App, float* Pos_set, float* Vel_max)
{
    if(Pos_set != NULL && Vel_max != NULL)
    {
        // 双环
        float Vel_set = PID_Calc(&Motor_App->Pid_P, Motor_App->Cur_Data.Position, *Pos_set);
        Motor_App->Pid_S.max_out = *Vel_max;
        Motor_App->Cur_Data.Output = PID_Calc(&Motor_App->Pid_S, Motor_App->Cur_Data.Speed, Vel_set);
        return 4;
    }
    else
    {
        if(Pos_set != NULL)
        {
            // 角度环
            Motor_App->Cur_Data.Output = PID_Calc(&Motor_App->Pid_P, Motor_App->Cur_Data.Position, *Pos_set);
            return 3;
        }
        else if(Vel_max != NULL)
        {
            // 速度环
            Motor_App->Cur_Data.Output = PID_Calc(&Motor_App->Pid_S, Motor_App->Cur_Data.Speed, *Vel_max);
            return 2;
        }
        else
        {
            Motor_App->Cur_Data.Output = 0;
            return 1;
        }
    }
}
